#include "dexterous_hand.h"
#include <iostream>
#include <thread>
#include <chrono>

#ifdef _WIN32
#include <windows.h>
#endif
#include <iostream>
#include <fcntl.h>
#include <io.h>

void statusCallback(const RS485Uplink& status) {
    static int count = 0;
    if (++count % 100 == 0) { // 每100次打印一次
        std::cout << "[" << DexterousHand::getCurrentTimeString() 
                  << "] 收到状态数据，关节0位置: " 
                  << status.jointParams[0].position << std::endl;
    }
}

int main() {
    #ifdef _WIN32
        // 设置Windows控制台为UTF-8模式
        SetConsoleOutputCP(CP_UTF8);
        SetConsoleCP(CP_UTF8);
        // 设置标准输出为UTF-8
        _setmode(_fileno(stdout), _O_U8TEXT);
        _setmode(_fileno(stderr), _O_U8TEXT);
    #endif
    std::string port = "COM8"; // 修改为您的串口
    
    // 创建灵巧手控制器
    DexterousHand hand(port);
    
    // 设置灵巧手ID
    hand.setHandId(0); // 默认ID为0
    
    // 初始化连接
    if (!hand.initialize()) {
        std::cerr << "灵巧手初始化失败" << std::endl;
        return 1;
    }
    
    // 设置状态回调
    hand.setStatusCallback(statusCallback);
    
    // 获取灵巧手参数
    UserData user_data;
    if (hand.getHandParameters(user_data)) {
        std::cout << "灵巧手参数:" << std::endl;
        std::cout << "  序列号: " << std::string(user_data.SN, 32) << std::endl;
        std::cout << "  名称: " << std::string(user_data.name, 32) << std::endl;
        std::cout << "  左右手: " << user_data.ev_hand << std::endl;
    }
    
    // 设置关节控制参数示例
    JointParamsDownlink joint_params = {};
    joint_params.kp = 100;
    joint_params.kd = 0;
    joint_params.position = 0;  // 目标位置
    joint_params.mode = 3;      // 位置控制模式
    joint_params.current_max = 0;
    
    // 为所有关节设置相同参数
    for (int i = 0; i < 21; ++i) {
        hand.setJointParams(i, joint_params);
    }
    
    // 启动实时控制
    if (!hand.startRealTimeControl(50)) {
        std::cerr << "启动实时控制失败" << std::endl;
        return 1;
    }
    
    std::cout << "灵巧手控制已启动，按Enter键退出..." << std::endl;
    
    // 运行一段时间后修改关节位置
    std::thread control_thread([&hand]() {
        std::this_thread::sleep_for(std::chrono::seconds(2));
        
        // 逐渐改变关节位置
        for (int pos = 0; pos < 1000; pos += 10) {
            JointParamsDownlink params = {};
            params.kp = 100;
            params.kd = 0;
            params.position = pos;
            params.mode = 3;
            params.current_max = 0;
            
            // 只控制第一个关节
            hand.setJointParams(0, params);
            
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    });
    
    // 等待用户输入
    std::cin.get();
    
    // 停止控制线程
    if (control_thread.joinable()) {
        control_thread.join();
    }
    
    // 关闭连接
    hand.close();
    
    std::cout << "程序结束" << std::endl;
    return 0;
}